#ifndef ROBORTS_DECISION_CAMERA_BEHAVIOR_H
#define ROBORTS_DECISION_CAMERA_BEHAVIOR_H

#include "io/io.h"

#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../proto/decision.pb.h"

#include "line_iterator.h"

namespace roborts_decision {
class CameraBehavior {
 public:
  CameraBehavior(ChassisExecutor* &chassis_executor,Blackboard* &blackboard) : 
              chassis_executor_(chassis_executor),blackboard_(blackboard) { }

  void Run() {
    auto enemypose = blackboard_->GetEnemyPoseFromCamera();
    auto mypose = blackboard_->GetRobotMapPose();
    if(enemypose.pose.position.x > mypose.pose.position.x) {
      enemypose.pose.orientation.z = 0;
      enemypose.pose.orientation.w = 1;
    }
    else {
      enemypose.pose.orientation.w = 0;
      enemypose.pose.orientation.z = -1;
    }
    double distance = blackboard_->GetDistance(enemypose, mypose);
    auto executor_state = Update();
    if (in_circle==0 && executor_state != BehaviorState::RUNNING && executor_state != BehaviorState::FAILURE) {
      if(distance < 1.0) {
        roborts_msgs::TwistAccel t;
        t.twist.angular.z = 1.5;
        chassis_executor_->Execute(t);
      }
      if(distance > 1.0) chassis_executor_->Execute(enemypose, Behaviortype::CAMERA);
    }
    else if(in_circle||executor_state == BehaviorState::FAILURE) {
      if(in_circle==0) {
        begin_time = ros::Time::now();
      }
      in_circle=1;
      roborts_msgs::TwistAccel t;
      t.twist.angular.z = 1.57;
      chassis_executor_->Execute(t);
      if(ros::Time::now().sec-begin_time.sec>=4)in_circle=0;
    }
  }

  void Cancel() {
    chassis_executor_->Cancel();
  }

  BehaviorState Update() {
    return chassis_executor_->Update();
  }

  ~CameraBehavior() = default;

 private:
  //! executor
  ChassisExecutor* const chassis_executor_;

  //! perception information
  Blackboard* const blackboard_;
  int in_circle;
  ros::Time begin_time;
};
}


#endif //ROBORTS_DECISION_CAMERA_BEHAVIOR_H
